#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "stm32f10x.h"
#include "stm32f10x_conf.h"
#include "timer.h"
#include "led.h"
#include "encoder.h"
#include "pid.h"

u8 SEND_BUF[10];

int main(void) {
    u8 time = 0;
    u8 angle = 0;
    u8 speed = 0;
    u8 x1, x2;
	int i;
    delay_init();
    PWM_Init();
    Encoder_Init();


#if USART
    uart_init(115200);

#else
    SBUS_USART1_Config(100000);
#endif

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    LED_Init();
    pid_init(&pid);

    TIM1->CCR4 = 145;

    while (1) {
#if USART
        //        // 通讯
        //        // 舵机
        //        if (USART_RX_STA & 0x8000) {
        //            angle = (int) (USART_RX_BUF[0] - '0');
        //            TIM1->CCR4 = 169 - angle * 8;
        //            // 速度
        //            speed = (int) (USART_RX_BUF[1] - '0') * 10;
        //            // 方向
        //            if (USART_RX_BUF[2] == '1') {
        //                pid.target = speed;
        //            } else if (USART_RX_BUF[2] == '0') {
        //                pid.target = 0;
        //            } else if (USART_RX_BUF[2] == '2') {
        //                pid.target = -1 * speed;
        //            }
        //            USART_RX_STA = 0;
        //        }
        //        set_motor(pid_count(&pid));
        // 自动寻线通
        if (USART_RX_STA & 0x8000) {
            angle = USART_RX_BUF[0];
            x1 = (USART_RX_BUF[1] - '0');
            x2 = (USART_RX_BUF[2] - '0');
            speed = (USART_RX_BUF[3]);
            //setangle
            if (speed == '1') {
                pid.target = 30;
            } else {
                pid.target = 0;
            }
            if (angle == '1') {
                // 向左
                TIM1->CCR4 = 145 + (x1 * 10 + x2) * 25 / 15;
            } else {
                TIM1->CCR4 = 145 - (x1 * 10 + x2) * 25 / 15;
            }
            USART_RX_STA = 0;
						        SEND_BUF[2] = (pid.speed % 10) + '0';
                    pid.speed /= 10;
                    SEND_BUF[1] = (pid.speed % 10) + '0';
                    pid.speed /= 10;
                    SEND_BUF[0] = (pid.speed % 10) + '0';
                    pid.speed = 0;
										SEND_BUF[3] = '\r';
										SEND_BUF[4] = '\n';
										for(i = 0;i < 5;i++){
												USART_SendData(USART1,SEND_BUF[i]);
												while(USART_GetFlagStatus(USART1,USART_FLAG_TC) == RESET){};
										}
        }
				            set_motor(pid_count(&pid));
#else
        // 遥控
        // 舵机
//        angle = (SBUS_Ch[2] - 200) / 32 + 120;
//        TIM1->CCR4 = angle;
//        // 电机
//        speed = (SBUS_Ch[3] - 200) / 32;
//        if (SBUS_Ch[5] == 1000) {
//            pid_reset(&pid);
//            set_motor(0);
//        } else if (SBUS_Ch[5] == 200) {
//            pid.target = speed;
//        } else if (SBUS_Ch[5]) {
//            pid.target = -1 * speed;
//        }

//        pid.output = pid_count(&pid);
//        set_motor(pid.output);

//				// 挡位
//        		120 - 170 左到右
//				if(SBUS_Ch[3] < 1000){
//						if(SBUS_Ch[6] == 200){
//								if(SBUS_Ch[5] == 200) TIM1->CCR4 = 121;
//								else if(SBUS_Ch[5] == 1000) TIM1->CCR4 = 129;
//								else if(SBUS_Ch[5] == 1800) TIM1->CCR4 = 137;
//						}
//						else if(SBUS_Ch[6] == 1800){
//								if(SBUS_Ch[5] == 200) TIM1->CCR4 = 145;
//				else if(SBUS_Ch[5] == 1000) TIM1->CCR4 = 153;
//				else if(SBUS_Ch[5] == 1800) TIM1->CCR4 = 161;
//						}
//				}
//
//				else{
//						TIM1->CCR4 = 169;
//				}

        if (SBUS_Ch[6] == 200) {
            if (SBUS_Ch[5] == 200) angle = 137;
            else if (SBUS_Ch[5] == 1000) angle = 145;
            else if (SBUS_Ch[5] == 1800) angle = 153;
        } else if (SBUS_Ch[6] == 1800) {
            if (SBUS_Ch[5] == 200) angle = 129;
            else if (SBUS_Ch[5] == 1000) angle = 145;
            else if (SBUS_Ch[5] == 1800) angle = 161;
        }
        TIM1->CCR4 = angle;

        speed = (SBUS_Ch[3] - 200) / 32;
        pid.target = speed;
        pid.output = pid_count(&pid);
        set_motor(pid.output);

#endif
        // 闪灯
        time++;
        if (time % 100 == 0) {
            LED0 = !LED0;
        }
        delay_ms(10);
    }

}







